"""
!/usr/bin/env python
-*- coding: utf-8 -*-
@CreateTime : 2024/7/2 10:10
@Author  :    AnimateX
@Contact :    animatex@163.com
@File    :    precision_core.py
@License :    Copyright © 2024 AnimateX. All rights reserved.
@Version :    cal_precision_VER0.1

------------------------------------------------------------------
# @Description:

------------------------------------------------------------------
"""
import math
import numpy as np
from sklearn.linear_model import LinearRegression, RANSACRegressor


class RMSLPrecision(object):
    def __init__(self):
        pass

    @staticmethod
    def calculate_rmse(true_values, measured_values):
        return np.sqrt(np.mean((true_values - measured_values) ** 2))

    @staticmethod
    def calculate_mae(true_values, measured_values):
        return np.mean(np.abs(true_values - measured_values))

    @staticmethod
    def calculate_mad(measured_values):
        median = np.median(measured_values)
        return np.median(np.abs(measured_values - median))

    @staticmethod
    def calculate_rep(true_values, measured_values):
        return np.mean(np.abs((true_values - measured_values) / true_values)) * 100

    @staticmethod
    def removeZerosAndOutliers(data, ratio=5):
        """
            [Description] remove outlier(5 * std) and zero points
        :param data:
        :param ratio:
        :return:
        """
        non_zero_data = data[data != 0]

        mean = np.mean(non_zero_data)
        std = np.std(non_zero_data)

        if std == 0:
            return non_zero_data

        lower_threshold = mean - ratio * std
        upper_threshold = mean + ratio * std

        filtered_data = non_zero_data[(non_zero_data > lower_threshold) & (non_zero_data < upper_threshold)]

        return filtered_data

    @staticmethod
    def calDistanceToPlane(point_cloud,
                           coef,
                           intercept,
                           en_filter=False):
        """

        :param point_cloud:
        :param coef:
        :param intercept:
        :param en_filter:
        :return:
        """
        a, b = coef
        c = 1
        d = intercept

        valid_mask = (point_cloud[:, 2] != 0)
        valid_points = point_cloud[valid_mask]

        numerator = np.abs(-a * valid_points[:, 0] - b * valid_points[:, 1] + c * valid_points[:, 2] - d)
        denominator = np.sqrt(a ** 2 + b ** 2 + c ** 2)
        distances = numerator / denominator

        ref_z = (-a * valid_points[:, 0] - b * valid_points[:, 1] - d) / c

        relative_error = np.where(ref_z != 0, distances / ref_z, np.inf)

        if en_filter:
            if np.all(distances == 0):
                rel_acc_val = distances
                rel_acc_per = np.array([np.inf])
            else:
                rel_acc_val = RMSLPrecision().removeZerosAndOutliers(distances, ratio=5)
                rel_acc_per = RMSLPrecision().removeZerosAndOutliers(relative_error, ratio=5)
        else:
            rel_acc_val = distances
            rel_acc_per = relative_error

        return rel_acc_val, rel_acc_per

    @staticmethod
    def fitPlaneRansacThenLeastSquares(point_cloud,
                                       residual_threshold=None,
                                       max_trials=100,
                                       seed=42):
        """

        :param point_cloud:
        :param residual_threshold:
        :param max_trials:
        :param seed:
        :return:
        """
        if point_cloud is None or point_cloud.size == 0:
            print(' [ERROR] point cloud data is empty.')
            return None, None

        x_matrix = point_cloud[:, :2]
        y = point_cloud[:, 2]

        # Use RANSAC to fitting
        ransac = RANSACRegressor(residual_threshold=residual_threshold, max_trials=max_trials, random_state=seed)
        ransac.fit(x_matrix, y)
        inlier_mask = ransac.inlier_mask_

        # Use Linear regression
        lr = LinearRegression()
        lr.fit(x_matrix[inlier_mask], y[inlier_mask])

        ransac_coef = (ransac.estimator_.coef_, ransac.estimator_.intercept_)
        lr_coef = (lr.coef_, lr.intercept_)

        return ransac_coef, lr_coef

    @staticmethod
    def fitPlaneNoOutliers(point_cloud,
                           max_iter=100,
                           outlier_ratio=5,
                           min_points=100,
                           seed=42):
        """

        :param point_cloud:
        :param max_iter:
        :param outlier_ratio:
        :param min_points:
        :param seed:
        :return:
        """
        if point_cloud is None or point_cloud.size == 0:
            print(' [ERROR] In point cloud data is empty.')
            return None, None, None

        ransac_coef = None
        lr_coef = None
        all_outlier_cnt = 0
        point_cloud_ori_cnt = len(point_cloud)

        for i in range(max_iter):
            # Fit the plane
            ransac_coef, lr_coef = RMSLPrecision().fitPlaneRansacThenLeastSquares(point_cloud, seed=seed)
            if lr_coef is None and ransac_coef is None:
                return None, None, None

            coef, intercept = lr_coef

            # Calculate the distance from plane to fit plane
            distances, rel_error = RMSLPrecision().calDistanceToPlane(point_cloud, coef, intercept)
            std_val = np.std(distances)

            # Identify the outliers
            outliers = distances > (outlier_ratio * std_val)  # ORBBEC

            # update PointCloud
            inliers = ~outliers
            new_point_cloud = point_cloud[inliers]

            num_outlier = np.sum(outliers)
            all_outlier_cnt += num_outlier
            if num_outlier == 0 or point_cloud.shape[0] < min_points:
                break

            # Check if point_cloud is empty
            if point_cloud.shape[0] < min_points:
                print(" [ERROR] Not enough inliers left after removing outliers. Stopping iterations.")
                break

            point_cloud = new_point_cloud

        if lr_coef is not None and point_cloud.shape[0] >= min_points:
            outlier_per = all_outlier_cnt / point_cloud_ori_cnt
            return ransac_coef, lr_coef, outlier_per
        else:
            return None, None, None

    @staticmethod
    def generatePointCloudByDepthData(depth_image,
                                      fx, fy, cx, cy,
                                      cen_ratio=1):
        """

        :param depth_image:
        :param fx:
        :param fy:
        :param cx:
        :param cy:
        :param cen_ratio:
        :return:
        """
        if fx is None or fy is None or cx is None or cy is None:
            return None, None

        # Remove invalid points (depth = 0)
        valid_mask = depth_image > 0

        # cal zero value point count.
        zero_val_cnt = np.sum(depth_image == 0)

        img_h = depth_image.shape[0]
        img_w = depth_image.shape[1]

        margin_w = int((1 - cen_ratio) / 2 * img_w)
        margin_h = int((1 - cen_ratio) / 2 * img_h)

        start_u, end_u = margin_w, img_w - margin_w
        start_v, end_v = margin_h, img_h - margin_h

        u, v = np.meshgrid(np.arange(start_u, end_u), np.arange(start_v, end_v))

        # generate point cloud data.
        x_data = (u - cx) * depth_image[start_v:end_v, start_u:end_u] / fx
        y_data = (v - cy) * depth_image[start_v:end_v, start_u:end_u] / fy
        z_data = depth_image[start_v:end_v, start_u:end_u]

        # Concatenate and reshape to (N, 3)
        point_cloud = np.column_stack((x_data[valid_mask[start_v:end_v, start_u:end_u]],
                                       y_data[valid_mask[start_v:end_v, start_u:end_u]],
                                       z_data[valid_mask[start_v:end_v, start_u:end_u]]))

        return point_cloud, zero_val_cnt

    @staticmethod
    def calculateImageRoiFov(fx, fy,
                             img_width,
                             img_height):
        """

        :param fx:
        :param fy:
        :param img_width:
        :param img_height:
        :return:
        """
        if fx <= 0 or fy <= 0:
            return None, None

        h_fov = 2 * math.degrees(math.atan(img_width / (2 * fx)))
        v_fov = 2 * math.degrees(math.atan(img_height / (2 * fy)))

        return h_fov, v_fov

    @staticmethod
    def extractOriginValidDepthImage(depth_img):
        """

        :param depth_img:
        :return:
        """
        # Find non-zero starting indices for each edge
        left_indices = np.argmax(depth_img != 0, axis=1)
        left_ref_value = int(np.mean(left_indices))

        right_indices = depth_img.shape[1] - np.argmax(np.flip(depth_img, axis=1) != 0, axis=1)
        right_ref_value = int(np.mean(right_indices[right_indices > 0]))  # Exclude the zero indices

        top_indices = np.argmax(depth_img != 0, axis=0)
        top_ref_value = int(np.mean(top_indices))

        bottom_indices = depth_img.shape[0] - np.argmax(np.flip(depth_img, axis=0) != 0, axis=0)
        bottom_ref_value = int(np.mean(bottom_indices[bottom_indices > 0]))  # Exclude the zero indices

        # Define the computation area based on the found indices
        valid_area = depth_img[top_ref_value: bottom_ref_value, left_ref_value:right_ref_value]

        return valid_area, left_ref_value, top_ref_value

    @staticmethod
    def getOpticalCenterDepthRoi(depth_img,
                                 fx=None, fy=None,
                                 cx=None, cy=None,
                                 min_ratio=0.5,
                                 max_ratio=0.9,
                                 cen_size_h=24,
                                 cen_size_v=24,
                                 use_img_cen=False,
                                 delBlank=True):
        """

        :param delBlank:
        :param depth_img:
        :param fx:
        :param fy:
        :param cx:
        :param cy:
        :param min_ratio:
        :param max_ratio:
        :param cen_size_h:
        :param cen_size_v:
        :param use_img_cen:
        :return:
        """
        # del black edge
        if delBlank:
            dp_img, s_x, s_y = RMSLPrecision().extractOriginValidDepthImage(depth_img)
        else:
            dp_img = depth_img
            s_x, s_y = 0, 0

        img_w = dp_img.shape[1]
        img_h = dp_img.shape[0]

        if use_img_cen or cx is None or cy is None:
            cx = img_w // 2
            cy = img_h // 2

        # update cx, cy
        cx = cx - s_x
        cy = cy - s_y

        min_w = int(min(cx * min_ratio, (img_w - cx) * min_ratio))
        max_w = int(min(cx * max_ratio, (img_w - cx) * max_ratio))
        min_h = int(min(cy * min_ratio, (img_h - cy) * min_ratio))
        max_h = int(min(cy * max_ratio, (img_h - cy) * max_ratio))

        min_s_x = int(cx - min_w)
        min_e_x = int(cx + min_w)
        min_s_y = int(cy - min_h)
        min_e_y = int(cy + min_h)

        max_s_x = int(cx - max_w)
        max_e_x = int(cx + max_w)
        max_s_y = int(cy - max_h)
        max_e_y = int(cy + max_h)

        # add center roi extract
        cen_s_x = max(0, int(cx - cen_size_h // 2))
        cen_e_x = min(int(cx + cen_size_h // 2), img_w - 1)
        cen_s_y = max(0, int(cy - cen_size_v // 2))
        cen_e_y = min(int(cy + cen_size_v // 2), img_h - 1)

        min_roi = dp_img[min_s_y: min_e_y, min_s_x: min_e_x]
        max_roi = dp_img[max_s_y: max_e_y, max_s_x: max_e_x]
        cen_roi = dp_img[cen_s_y: cen_e_y, cen_s_x: cen_e_x]
        param_list = [min_s_x, min_s_y, max_s_x, max_s_y, cen_s_x, cen_s_y, cx, cy]

        if fx is None or fy is None:
            calib_fov_res = {
                'h_val': None,
                'h_max': None,
                'h_min': None,
                'h_cen': None,
                'v_val': None,
                'v_max': None,
                'v_min': None,
                'v_cen': None,
            }
        else:
            val_h_fov, val_v_fov = RMSLPrecision().calculateImageRoiFov(fx, fy, dp_img.shape[1], dp_img.shape[0])
            max_h_fov, max_v_fov = RMSLPrecision().calculateImageRoiFov(fx, fy, max_roi.shape[1], max_roi.shape[0])
            min_h_fov, min_v_fov = RMSLPrecision().calculateImageRoiFov(fx, fy, min_roi.shape[1], min_roi.shape[0])
            cen_h_fov, cen_v_fov = RMSLPrecision().calculateImageRoiFov(fx, fy, cen_roi.shape[1], cen_roi.shape[0])

            calib_fov_res = {
                'h_val': val_h_fov,
                'h_max': max_h_fov,
                'h_min': min_h_fov,
                'h_cen': cen_h_fov,
                'v_val': val_v_fov,
                'v_max': max_v_fov,
                'v_min': min_v_fov,
                'v_cen': cen_v_fov,
            }

        return calib_fov_res, min_roi, max_roi, cen_roi, param_list

    @staticmethod
    def calCoefOfFittingPlane(point_cloud,
                              seed=42,
                              en_remove_outliers=True):
        """

        :param point_cloud:
        :param seed:
        :param en_remove_outliers:
        :return:
        """
        outlier_per = 0
        if en_remove_outliers:
            ransac_coef, lr_coef, outlier_per = RMSLPrecision().fitPlaneNoOutliers(point_cloud, seed=seed)
        else:
            ransac_coef, lr_coef = RMSLPrecision().fitPlaneRansacThenLeastSquares(point_cloud, seed=seed)

        return ransac_coef, lr_coef, outlier_per

    @staticmethod
    def calDistanceFromTarToFittedPlane(point_cloud,
                                        lr_coef,
                                        en_filter=True):
        """

        :param point_cloud:
        :param lr_coef:
        :param en_filter:
        :return:
        """
        if lr_coef is None:
            print(' [ERROR] Invalid coef, failed fitting plane data.')
            return None, None
        else:
            coef, intercept = lr_coef
            rel_acc_val, rel_acc_per = RMSLPrecision().calDistanceToPlane(point_cloud,
                                                                          coef,
                                                                          intercept,
                                                                          en_filter=en_filter)
            return rel_acc_val, rel_acc_per

    @staticmethod
    def calMeanPrecisionByORBBECMethod(depth_img,
                                       measure_dis,
                                       cx, cy,
                                       roi_w=24,
                                       roi_h=24,
                                       cen_size_h=24,
                                       cen_size_v=24,
                                       min_ratio=0.5,
                                       max_ratio=0.9,
                                       en_img_cen=False,
                                       en_precise_acc=False):
        """

        :param max_ratio:
        :param min_ratio:
        :param depth_img:
        :param measure_dis:
        :param cx:
        :param cy:
        :param roi_w:
        :param roi_h:
        :param cen_size_h:
        :param cen_size_v:
        :param en_img_cen:
        :param en_precise_acc:
        :return:
        """
        if en_precise_acc:
            measure_dis = measure_dis * 10
        fx = None
        fy = None
        _, min_roi, max_roi, cen_roi, param_list = RMSLPrecision().getOpticalCenterDepthRoi(depth_img,
                                                                                            fx, fy,
                                                                                            cx, cy,
                                                                                            min_ratio=min_ratio,
                                                                                            max_ratio=max_ratio,
                                                                                            cen_size_h=cen_size_h,
                                                                                            cen_size_v=cen_size_v,
                                                                                            use_img_cen=en_img_cen,
                                                                                            delBlank=True)
        roi_81 = max_roi
        roi_25 = min_roi

        cx = int(param_list[6])
        cy = int(param_list[7])
        cx_max = int(cx - param_list[2])
        cy_max = int(cy - param_list[3])

        rois = {
            'roi_c': roi_81[cy_max - roi_h // 2:cy_max + roi_h // 2, cx_max - roi_w // 2:cx_max + roi_w // 2],
            'roi_l': roi_81[cy_max - roi_h // 2:cy_max + roi_h // 2, :roi_w],
            'roi_r': roi_81[cy_max - roi_h // 2:cy_max + roi_h // 2, -roi_w:],
            'roi_t': roi_81[:roi_h, cx_max - roi_w // 2:cx_max + roi_w // 2],
            'roi_b': roi_81[-roi_h:, cx_max - roi_w // 2:cx_max + roi_w // 2],
            'roi_81': roi_81,
            'roi_25': roi_25,
            'roi_cen': cen_roi
        }

        roi_dis = {area: np.mean(RMSLPrecision().removeZerosAndOutliers(roi)) for area, roi in rois.items() if
                   roi.size > 0}
        abs_acc = {area: measure_dis - dis for area, dis in roi_dis.items()}
        rmse = {area: RMSLPrecision().calculate_rmse(dis, measure_dis) for area, dis in roi_dis.items()}

        abs_acc_res = {
            'avg_roi_c': np.mean(abs_acc['roi_c']),
            'avg_roi_l': np.mean(abs_acc['roi_l']),
            'avg_roi_r': np.mean(abs_acc['roi_r']),
            'avg_roi_t': np.mean(abs_acc['roi_t']),
            'avg_roi_b': np.mean(abs_acc['roi_b']),
            'avg_roi_25': np.mean(abs_acc['roi_25']),
            'avg_roi_81': np.mean(abs_acc['roi_81']),
            'avg_roi_cen': np.mean(abs_acc['roi_cen']),
            'min_roi_c': np.min(abs_acc['roi_c']),
            'min_roi_l': np.min(abs_acc['roi_l']),
            'min_roi_r': np.min(abs_acc['roi_r']),
            'min_roi_t': np.min(abs_acc['roi_t']),
            'min_roi_b': np.min(abs_acc['roi_b']),
            'min_roi_25': np.min(abs_acc['roi_25']),
            'min_roi_81': np.min(abs_acc['roi_81']),
            'min_roi_cen': np.min(abs_acc['roi_cen']),
            'max_roi_c': np.max(abs_acc['roi_c']),
            'max_roi_l': np.max(abs_acc['roi_l']),
            'max_roi_r': np.max(abs_acc['roi_r']),
            'max_roi_t': np.max(abs_acc['roi_t']),
            'max_roi_b': np.max(abs_acc['roi_b']),
            'max_roi_25': np.max(abs_acc['roi_25']),
            'max_roi_81': np.max(abs_acc['roi_81']),
            'max_roi_cen': np.max(abs_acc['roi_cen']),
            'std_roi_c': np.std(abs_acc['roi_c']),
            'std_roi_l': np.std(abs_acc['roi_l']),
            'std_roi_r': np.std(abs_acc['roi_r']),
            'std_roi_t': np.std(abs_acc['roi_t']),
            'std_roi_b': np.std(abs_acc['roi_b']),
            'std_roi_25': np.std(abs_acc['roi_25']),
            'std_roi_81': np.std(abs_acc['roi_81']),
            'std_roi_cen': np.std(abs_acc['roi_b']),
            'var_roi_c': np.var(abs_acc['roi_c']),
            'var_roi_l': np.var(abs_acc['roi_l']),
            'var_roi_r': np.var(abs_acc['roi_r']),
            'var_roi_t': np.var(abs_acc['roi_t']),
            'var_roi_b': np.var(abs_acc['roi_b']),
            'var_roi_25': np.var(abs_acc['roi_25']),
            'var_roi_81': np.var(abs_acc['roi_81']),
            'var_roi_cen': np.var(abs_acc['roi_cen']),
            'rmse_roi_c': rmse['roi_c'],
            'rmse_roi_l': rmse['roi_l'],
            'rmse_roi_r': rmse['roi_r'],
            'rmse_roi_t': rmse['roi_t'],
            'rmse_roi_b': rmse['roi_b'],
            'rmse_roi_25': rmse['roi_25'],
            'rmse_roi_81': rmse['roi_81'],
            'rmse_roi_cen': rmse['roi_cen'],
        }

        return abs_acc_res

    @staticmethod
    def calRelativePrecision(depth_img,
                             fx, fy,
                             cx, cy,
                             seed=42,
                             cen_size_h=24,
                             cen_size_v=24,
                             min_ratio=0.5,
                             max_ratio=0.9,
                             use_img_cen=False,
                             en_filter=False):

        """ [1] Get valid region """
        fov_res, min_roi, max_roi, cen_roi, param_list = RMSLPrecision().getOpticalCenterDepthRoi(depth_img,
                                                                                                  fx, fy,
                                                                                                  cx, cy,
                                                                                                  min_ratio=min_ratio,
                                                                                                  max_ratio=max_ratio,
                                                                                                  cen_size_h=cen_size_h,
                                                                                                  cen_size_v=cen_size_v,
                                                                                                  use_img_cen=use_img_cen,
                                                                                                  delBlank=True)
        depth_cen = cen_roi
        depth_min = min_roi
        depth_max = max_roi
        cx = param_list[6]
        cy = param_list[7]

        cx_min = cx - param_list[0]
        cy_min = cy - param_list[1]
        cx_max = cx - param_list[2]
        cy_max = cy - param_list[3]
        cx_cen = cx - param_list[4]
        cy_cen = cy - param_list[5]

        """ [2] Generate point cloud"""
        point_cloud_min, zero_val_cnt_min = RMSLPrecision().generatePointCloudByDepthData(depth_min,
                                                                                          fx, fy,
                                                                                          cx_min,
                                                                                          cy_min)

        point_cloud_max, zero_val_cnt_max = RMSLPrecision().generatePointCloudByDepthData(depth_max,
                                                                                          fx, fy,
                                                                                          cx_max,
                                                                                          cy_max)

        point_cloud_cen, zero_val_cnt_cen = RMSLPrecision().generatePointCloudByDepthData(depth_cen,
                                                                                          fx, fy,
                                                                                          cx_cen,
                                                                                          cy_cen)

        """ [3] Get coef """
        ransac_coef, lr_coef, outlier_per = RMSLPrecision().calCoefOfFittingPlane(point_cloud_max, seed=seed)

        if ransac_coef is None or lr_coef is None:
            print("fitting coeffs(ransac_coeff and lr_coeff) is none!")
            relative_acc_val = {
                'avg_val_roi_25':  0,
                'avg_val_roi_81':  0,
                'avg_val_roi_cen': 0,
                'min_val_roi_25':  0,
                'min_val_roi_81':  0,
                'min_val_roi_cen': 0,
                'max_val_roi_25':  0,
                'max_val_roi_81':  0,
                'max_val_roi_cen': 0,
                'std_val_roi_25':  0,
                'std_val_roi_81':  0,
                'std_val_roi_cen': 0,
                'var_val_roi_25':  0,
                'var_val_roi_81':  0,
                'var_val_roi_cen': 0,
            }

            relative_acc_per = {
                'avg_per_roi_25':  0,
                'avg_per_roi_81':  0,
                'avg_per_roi_cen': 0,
                'min_per_roi_25':  0,
                'min_per_roi_81':  0,
                'min_per_roi_cen': 0,
                'max_per_roi_25':  0,
                'max_per_roi_81':  0,
                'max_per_roi_cen': 0,
                'std_per_roi_25':  0,
                'std_per_roi_81':  0,
                'std_per_roi_cen': 0,
                'var_per_roi_25':  0,
                'var_per_roi_81':  0,
                'var_per_roi_cen': 0,
            }

            return relative_acc_val, relative_acc_per, fov_res, outlier_per

        """ [4] Cal acc """
        rel_acc_val_min, rel_acc_per_min = RMSLPrecision().calDistanceFromTarToFittedPlane(point_cloud_min,
                                                                                           lr_coef,
                                                                                           en_filter=en_filter)
        rel_acc_val_max, rel_acc_per_max = RMSLPrecision().calDistanceFromTarToFittedPlane(point_cloud_max,
                                                                                           lr_coef,
                                                                                           en_filter=en_filter)
        rel_acc_val_cen, rel_acc_per_cen = RMSLPrecision().calDistanceFromTarToFittedPlane(point_cloud_cen,
                                                                                           lr_coef,
                                                                                           en_filter=en_filter)
        rel_acc_res = {
            'outlier_percent': outlier_per,
            'fov_h_val': fov_res['h_val'],
            'fov_h_max': fov_res['h_max'],
            'fov_h_min': fov_res['h_min'],
            'fov_h_cen': fov_res['h_cen'],
            'fov_v_val': fov_res['v_val'],
            'fov_v_max': fov_res['v_max'],
            'fov_v_min': fov_res['v_min'],
            'fov_v_cen': fov_res['v_cen'],
            'avg_roi_25': np.mean(rel_acc_val_min),
            'avg_roi_81': np.mean(rel_acc_val_max),
            'avg_roi_cen': np.mean(rel_acc_val_cen),
            'min_roi_25': np.min(rel_acc_val_min),
            'min_roi_81': np.min(rel_acc_val_max),
            'min_roi_cen': np.min(rel_acc_val_cen),
            'max_roi_25': np.max(rel_acc_val_min),
            'max_roi_81': np.max(rel_acc_val_max),
            'max_roi_cen': np.max(rel_acc_val_cen),
            'std_roi_25': np.std(rel_acc_val_min),
            'std_roi_81': np.std(rel_acc_val_max),
            'std_roi_cen': np.std(rel_acc_val_cen),
            'var_roi_25': np.var(rel_acc_val_min),
            'var_roi_81': np.var(rel_acc_val_max),
            'var_roi_cen': np.var(rel_acc_val_cen),
            'avg_per_roi_25': np.mean(rel_acc_per_min),
            'avg_per_roi_81': np.mean(rel_acc_per_max),
            'avg_per_roi_cen': np.mean(rel_acc_per_cen),
            'min_per_roi_25': np.min(rel_acc_per_min),
            'min_per_roi_81': np.min(rel_acc_per_max),
            'min_per_roi_cen': np.min(rel_acc_per_cen),
            'max_per_roi_25': np.max(rel_acc_per_min),
            'max_per_roi_81': np.max(rel_acc_per_max),
            'max_per_roi_cen': np.max(rel_acc_per_cen),
            'std_per_roi_25': np.std(rel_acc_per_min),
            'std_per_roi_81': np.std(rel_acc_per_max),
            'std_per_roi_cen': np.std(rel_acc_per_cen),
            'var_per_roi_25': np.var(rel_acc_per_min),
            'var_per_roi_81': np.var(rel_acc_per_max),
            'var_per_roi_cen': np.var(rel_acc_per_cen),
        }

        # filename = f'min_roi_dis_{np.mean(abs_acc_min) / 10:0>10.5f}mm.txt'
        # np.savetxt(filename, point_cloud_max, delimiter=" ", header="X Y Z", comments="")

        return rel_acc_res

    @staticmethod
    def calEcovasRobotPointCloudPrecision(depth_image, distance, fx, fy, cx, cy, num_blocks=3, fraction=40):
        """

        :param depth_image:
        :param distance:
        :param fx:
        :param fy:
        :param cx:
        :param cy:
        :param num_blocks:
        :param fraction:
        :return:
        """
        img_h, img_w = depth_image.shape
        block_h, block_w = img_h // num_blocks, img_w // num_blocks
        roi_w = img_w // fraction
        roi_h = img_h // fraction
        results = {}

        for i in range(num_blocks):
            for j in range(num_blocks):
                key_str = ''
                if j == 0:
                    key_str += 'L'
                elif j == 1:
                    key_str += 'C'
                elif j == 2:
                    key_str += 'R'

                if i == 0:
                    key_str += 'U'
                elif i == 1:
                    key_str += 'C'
                elif i == 2:
                    key_str += 'D'

                # Find roi start and end
                x_start = j * block_w + (block_w - roi_w) // 2
                x_end = x_start + roi_w
                y_start = i * block_h + (block_h - roi_h) // 2
                y_end = y_start + roi_h

                small_depth = depth_image[y_start:y_end, x_start:x_end]

                cx_new = cx - x_start
                cy_new = cy - y_start

                point_cloud, _ = RMSLPrecision().generatePointCloudByDepthData(small_depth, fx, fy, cx_new, cy_new)

                if point_cloud[:, 2].size > 0:
                    avg_z = np.mean(np.abs(point_cloud[:, 2] - distance))
                    max_z = np.max(np.abs(point_cloud[:, 2] - distance))
                    min_z = np.min(np.abs(point_cloud[:, 2] - distance))
                    std_z = np.std(np.abs(point_cloud[:, 2] - distance))
                    var_z = np.var(np.abs(point_cloud[:, 2] - distance))
                else:
                    print("No PointCloud Found! Check!")
                    avg_z = -1.0
                    max_z = -1.0
                    min_z = -1.0
                    std_z = -1.0
                    var_z = -1.0
                results[key_str + '_avg'] = avg_z
                results[key_str + '_max'] = max_z
                results[key_str + '_min'] = min_z
                results[key_str + '_std'] = std_z
                results[key_str + '_var'] = var_z

        return results

    @staticmethod
    def calEcovasRobotPointCloudPrecisionByFit(depth_image,
                                               fx, fy,
                                               cx, cy,
                                               seed=42,
                                               cen_size_h=24,
                                               cen_size_v=24,
                                               num_blocks=3,
                                               fraction=40,
                                               min_ratio=0.5,
                                               max_ratio=0.9,
                                               use_img_cen=False,
                                               en_filter=False,
                                               en_special3x3=True):
        """

        :param en_special3x3:
        :param max_ratio:
        :param min_ratio:
        :param depth_image:
        :param fx:
        :param fy:
        :param cx:
        :param cy:
        :param seed:
        :param cen_size_h:
        :param cen_size_v:
        :param num_blocks:
        :param fraction:
        :param use_img_cen:
        :param en_filter:
        :return:
        """

        """ [1] Get valid region """
        fov_res, min_roi, max_roi, cen_roi, param_list = RMSLPrecision().getOpticalCenterDepthRoi(depth_image,
                                                                                                  fx, fy,
                                                                                                  cx, cy,
                                                                                                  min_ratio=min_ratio,
                                                                                                  max_ratio=max_ratio,
                                                                                                  cen_size_h=cen_size_h,
                                                                                                  cen_size_v=cen_size_v,
                                                                                                  use_img_cen=use_img_cen)
        depth_max = max_roi
        cx = param_list[6]
        cy = param_list[7]

        cx_max = cx - param_list[2]
        cy_max = cy - param_list[3]

        point_cloud_max, zero_val_cnt_max = RMSLPrecision().generatePointCloudByDepthData(depth_max,
                                                                                          fx, fy,
                                                                                          cx_max,
                                                                                          cy_max)

        """ [3] Get coef """
        ransac_coef, lr_coef, outlier_per = RMSLPrecision().calCoefOfFittingPlane(point_cloud_max, seed=seed)

        if en_special3x3:
            depth_img = max_roi
        else:
            depth_img = depth_image

        img_h, img_w = depth_img.shape
        block_h, block_w = img_h // num_blocks, img_w // num_blocks
        roi_w = img_w // fraction
        roi_h = img_h // fraction

        coef, intercept = lr_coef
        results = {
            'coef': coef.tolist(),
            'intercept': intercept
        }

        results = {}

        for i in range(num_blocks):
            for j in range(num_blocks):
                key_str = ''
                if j == 0:
                    key_str += 'L'
                elif j == 1:
                    key_str += 'C'
                elif j == 2:
                    key_str += 'R'

                if i == 0:
                    key_str += 'U'
                elif i == 1:
                    key_str += 'C'
                elif i == 2:
                    key_str += 'D'

                # Find roi start and end
                x_start = j * block_w + (block_w - roi_w) // 2
                x_end = x_start + roi_w
                y_start = i * block_h + (block_h - roi_h) // 2
                y_end = y_start + roi_h

                small_depth = depth_img[y_start:y_end, x_start:x_end]

                if en_special3x3:
                    cx_new = cx_max - x_start
                    cy_new = cy_max - y_start
                else:
                    cx_new = cx - x_start
                    cy_new = cy - y_start

                point_cloud, _ = RMSLPrecision().generatePointCloudByDepthData(small_depth, fx, fy, cx_new, cy_new)
                rel_acc, _ = RMSLPrecision().calDistanceFromTarToFittedPlane(point_cloud, lr_coef, en_filter=en_filter)

                acc_val = {
                    'avg': np.mean(rel_acc),
                    'min': np.min(rel_acc),
                    'max': np.max(rel_acc),
                    'std': np.std(rel_acc),
                    'var': np.var(rel_acc),
                }

                results[key_str + '_avg'] = acc_val['avg']
                results[key_str + '_min'] = acc_val['min']
                results[key_str + '_max'] = acc_val['max']
                results[key_str + '_std'] = acc_val['std']
                results[key_str + '_var'] = acc_val['var']

        return results
